//Copyright 2012 Trevin Liberty
//GNU GPL v3
//Author: Trevin Liberty

#include "StateFunctions.h"

#include "../../Structs.h"

#include <math.h>

#include <iostream>
using namespace std;

void moveUnit(Entity* e) {
	Map* map = e->getMap();

	Point<float> pos = e->getPosition();

	Point<int> destIndex = e->getCurrentDestination();
	Point<int> dest;
	dest.x = map->mapArray[destIndex.x][destIndex.y]->x;
	dest.y = map->mapArray[destIndex.x][destIndex.y]->y;

	Point<int> cMapCellIndex = e->getPosIndex();
	Point<int> mapPos;
	mapPos.x = map->mapArray[cMapCellIndex.x][cMapCellIndex.y]->x;
	mapPos.y = map->mapArray[cMapCellIndex.x][cMapCellIndex.y]->y;

	Point<int> goal = e->getGoal();

	Point<float> vector(dest.x - pos.x, dest.y - pos.y);
	vector.normalize();

	float offset = 5.0;
	
	//If at dest
	if(pos.x > dest.x - 2.0 && pos.x < dest.x + 2.0 &&
		pos.y > dest.y - 2.0 && pos.y < dest.y + 2.0) {
		e->setCurrentDestination(e->getPath()->front());
		e->getPath()->pop_front();
	}
	//if dest is not occupied, move
	else if(map->mapArray[destIndex.x][destIndex.y]->doohickey == NULL ||
		map->mapArray[destIndex.x][destIndex.y]->doohickey->getID() == e->getID()) {
		e->addToX(vector.x*e->getSpeed());
		e->addToY(vector.y*e->getSpeed());
		pos.x = e->getPosition().x;
		pos.y = e->getPosition().y;
		e->setDirection(vector);
		e->setNotBlocked();
		moveEntityMapCell(e, Point<int>(destIndex.x, destIndex.y));
	}
	//if location is occupied, set blocked, don't move
	else {
		e->setBlocked();
		return;
	}

	//If inside current mapCell
	//ie not between two cells
	if(pos.x > mapPos.x - 2.5 && pos.x < mapPos.x + 2.5 &&
		pos.y > mapPos.y - 2.5 && pos.y < mapPos.y + 2.5) {
			e->setArrived(true);
	}
	else
		e->setArrived(false);
}

void moveEntityMapCell(Entity* e, Point<int> dest) {
	Map* map = e->getMap();

	if(dest.x < 0 || dest.y < 0 || 
		dest.x >= map->numTilesX || 
		dest.y >= map->numTilesY) {
			cout << "ERROR moveEntityMapCell";
			return;
	}

	Point<int> pos = e->getMapCellPosition();

	if(dest.x == pos.x && dest.y == pos.y)
		return;

	//if(map->mapArray[pos.x][pos.y]->entity != NULL &&
	//	map->mapArray[pos.x][pos.y]->entity->getID() == e->getID()) {
	map->mapArray[pos.x][pos.y]->doohickey = NULL;
	/*}*/

	map->mapArray[dest.x][dest.y]->doohickey = e;
	e->setMapCellPos(dest);
}

Doohickey* someEnemyInRange(Entity* e, int range) {
	Entity* target = NULL;
	Doohickey* d;
	Map* map = e->getMap();

	Point<int> index = e->getPosition()/32;

	int k = range;

	for(int i = k*-1; i <= k; i++) {
		for(int j = k*-1; j <= k; j++) {
			if(index.x+i < 0 || index.x+i >= map->numTilesX ||
				index.y+j < 0 || index.y+j >= map->numTilesY)
				continue;
			d = map->mapArray[index.x+i][index.y+j]->doohickey;
			if(d != NULL && (d->getTeam() != e->getTeam()) && (d->isAEntity())) {
				Point<int> tp = d->getPosIndex();
				if(sqrt(pow((index.x - tp.x), 2.0) + pow((index.y - tp.y), 2.0)) <= range)
					return map->mapArray[index.x+i][index.y+j]->doohickey;
			}
		}
	}
	return NULL;
}